//
//  PoseDector.swift
//  SitPosition
//
//  Created by apple on 2019/2/3.
//  Copyright © 2019 apple. All rights reserved.
//

import Foundation


struct PoseDector {
    
    // 鼻子
    private var Nosex: Double = 0
    private var Nosey: Double = 0
    
    // 左眼
    private var LEyex: Double = 0
    private var LEyey: Double = 0
    
    // 右眼
    private var REyex: Double = 0
    private var REyey: Double = 0
    
    // 左耳
    private var LEarx: Double = 0
    private var LEary: Double = 0
    
    // 右耳
    private var REarx: Double = 0
    private var REary: Double = 0
    
    // 左肩
    private var LShoulderx: Double = 0
    private var LShouldery: Double = 0
    
    // 右肩
    private var RShoulderx: Double = 0
    private var RShouldery: Double = 0
    
    // 脖子
    private var Neckx: Double = 0
    private var Necky: Double = 0
    
    // 检测敏感度
    private var Sensitivity: Double = 1.0
    
    // 根据Human检测坐姿
    public mutating func detect(by human: Human) -> PoseType {
        // 鼻子
        Nosex = 0
        Nosey = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.Nose.rawValue] {
            Nosex = Double(bp.x)
            Nosey = Double(bp.y)
        }
        
        // 左眼
        LEyex = 0
        LEyey = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.LEye.rawValue] {
            LEyex = Double(bp.x)
            LEyey = Double(bp.y)
        }
        
        // 右眼
        REyex = 0
        REyey = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.REye.rawValue] {
            REyex = Double(bp.x)
            REyey = Double(bp.y)
        }
        
        // 左耳
        LEarx = 0
        LEary = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.LEar.rawValue] {
            LEarx = Double(bp.x)
            LEary = Double(bp.y)
        }
        
        // 右耳
        REarx = 0
        REary = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.REar.rawValue] {
            REarx = Double(bp.x)
            REary = Double(bp.y)
        }
        
        // 左肩
        LShoulderx = 0
        LShouldery = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.LShoulder.rawValue] {
            LShoulderx = Double(bp.x)
            LShouldery = Double(bp.y)
        }
        
        // 右肩
        RShoulderx = 0
        RShouldery = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.RShoulder.rawValue] {
            RShoulderx = Double(bp.x)
            RShouldery = Double(bp.y)
        }
        
        // 脖子
        Neckx = 0
        Necky = 0
        if let bp: BodyPart = human.bodyParts[CocoPart.Neck.rawValue] {
            Neckx = Double(bp.x)
            Necky = Double(bp.y)
        }
        
        // 没有检测到人体关键点位：无类型坐姿错误
        if (REyey == 0 && LEyey == 0 && REyex == 0 && LEyex == 0) ||
           (RShoulderx == 0 && RShouldery == 0 && LShoulderx == 0 && LShouldery == 0) {
            return PoseType.unknownSlant;
        }
        
        /**
         * 计算姿态
         */
        let config: UserConfigs? = UserConfigs.current
        
        let angleFactor = Double(config?.angleValue ?? UserConfigs.defaultAngle)
        
        // 身体倾斜
        var threshold: Int = Preferences.bodySlantThreshold ?? StudyAngleAdvSetting.Default.bodySlant
        var result: PoseType = getbodySlantingAngle(angleFactor * Double(threshold))
        if result != PoseType.ok {
            return result
        }
        // 低头、仰头
        threshold = Preferences.headLowUpThreshold ?? StudyAngleAdvSetting.Default.headLowUp
        result = getLowUpHeadAngle(angleFactor * Double(threshold))
        if result != PoseType.ok {
            return result
        }
        // 头部倾斜
        let headLeftSlantThreshold = Preferences.headLeftThreshold ?? StudyAngleAdvSetting.Default.headLR
        let headRightSlantThreshold = Preferences.headRightThreshold ?? StudyAngleAdvSetting.Default.headLR
        result = getHeadSlantingAngle(angleFactor * Double(headLeftSlantThreshold),
                                      angleFactor * Double(headRightSlantThreshold))
        if result != PoseType.ok {
            return result
        }
//        // 头部旋转
//        threshold = Preferences.headRotateThreshold ?? StudyAngleAdvSetting.Default.headRotate
//        result = getHeadRotateAngle(angleFactor * Double(threshold))
//        if result != PoseType.ok {
//            return result
//        }
        return PoseType.ok
    }
    
    // 检测头部左歪、右歪
    public func getHeadSlantingAngle(_ LAnglethreshold: Double, _ RAnglethreshold: Double) -> PoseType {
        let Lthreshold: Double = LAnglethreshold*Sensitivity
        let Rthreshold: Double = RAnglethreshold*Sensitivity
        if (REyey != 0 && LEyey != 0 && REyex != 0 && LEyex != 0) {
            let LeyeReyeline_Angle: Double = atan2(REyey - LEyey, REyex - LEyex) / (3.14 / 180.0)
            if (LeyeReyeline_Angle>0 && 180-LeyeReyeline_Angle > Rthreshold) {
                return PoseType.headerSlantL
            }
            if (LeyeReyeline_Angle < 0 && 180+LeyeReyeline_Angle > Lthreshold) {
                return PoseType.headerSlantR
            }
        }
        return PoseType.ok;
    }
    
    //头左旋转,头右旋转
    public func getHeadRotateAngle(_ Anglethreshold: Double) -> PoseType {
        let threshold: Double = Anglethreshold*Sensitivity;
        
        if (LEarx == 0 && REarx != 0) {
            return PoseType.headerSlantRotateL.reflection
        }
        if (REarx == 0 && LEarx != 0) {
            return PoseType.headerSlantRotateR.reflection
        }
        if (LEarx != 0 && REarx != 0 && LEyex != 0 && REyex != 0) {
            let LLine: Double = abs(LEarx-LEyex)
            let RLine: Double = abs(REarx-REyex)
            if (LLine < RLine) {
                let Rotate: Double = (acos(LLine/RLine) / (3.14 / 180))/2
                if (Rotate > threshold) {
                    return PoseType.headerSlantRotateR
                }
            }
            if (LLine > RLine) {
                let Rotate: Double = (acos(RLine/LLine) / (3.14 / 180))/2
                if (Rotate > threshold) {
                    return PoseType.headerSlantRotateL
                }
            }
        }
        return PoseType.ok;
    }
    
    
    // 低头、抬头（耳朵眼睛鼻子之间的夹角）
    public func getLowUpHeadAngle(_ Maxthreshold: Double) -> PoseType {
        var LAngle: Double = 0
        var RAngle: Double = 0
        if (LEarx != 0 && LEary != 0 && LEyey != 0 && LEyex != 0) {
            // 眼睛到耳朵的直线斜率
            let KLearLeye: Double = (LEyey-LEary) / (LEyex-LEarx)
            // 眼睛到鼻子的直线斜率
            let KNoseLeye: Double = (LEyey-Nosey ) / (LEyex-Nosex)
            
            LAngle = atan(abs(KLearLeye-KNoseLeye) / (1 + KLearLeye * KNoseLeye)) / (3.14 / 180)
        }
        
        if (REarx != 0 && REary != 0 && REyey != 0 && REyex != 0) {
            // 眼睛到耳朵的直线斜率
            let KRearReye: Double = (REyey-REary  ) / (REyex-REarx)
            // 眼睛到鼻子的直线斜率
            let KNoseReye: Double = (REyey-Nosey) / (REyex-Nosex)
            
            RAngle = atan(abs(KNoseReye-KRearReye) / (1 + KRearReye * KNoseReye)) / (3.14 / 180);
        }
        let Lthreshold: Double = Maxthreshold*Sensitivity
        
        RAngle = (180 - abs(RAngle))
        LAngle = (180 - abs(LAngle))
        if (RAngle + LAngle)/2 > Lthreshold {
            return PoseType.headerSlantLow
        }
        if RShoulderx != 0 && RShouldery != 0 {
            let ShoulderToNeck: Double =  sqrt((abs(RShoulderx-Neckx)*abs(RShoulderx-Neckx))+(abs(RShouldery-Necky)*abs(RShouldery-Necky)))
            let NoseToNeck: Double = sqrt((abs(Nosex-Neckx)*abs(Nosex-Neckx))+(abs(Nosey-Necky)*abs(Nosey-Necky)))
            if NoseToNeck < (ShoulderToNeck/2) {
                return PoseType.headerSlantLow
            }
        }
        if LShoulderx != 0 && LShouldery != 0 {
            let ShoulderToNeck: Double = sqrt((abs(LShoulderx-Neckx)*abs(LShoulderx-Neckx))+(abs(LShouldery-Necky)*abs(LShouldery-Necky)))
            let NoseToNeck: Double = sqrt((abs(Nosex-Neckx)*abs(Nosex-Neckx))+(abs(Nosey-Necky)*abs(Nosey-Necky)))
            if NoseToNeck < (ShoulderToNeck/2) {
                return PoseType.headerSlantLow
            }
        }
        return PoseType.ok;
    }
    
    // 身体左倾,身体右倾
    public func getbodySlantingAngle(_ Anglethreshold: Double) -> PoseType {
        let threshold: Double = Anglethreshold*Sensitivity
        if (RShoulderx != 0 && RShouldery != 0 && LShoulderx != 0 && LShouldery != 0) {
            let line_Angle: Double = atan2(RShouldery - LShouldery, RShoulderx - LShoulderx) / (3.14 / 180);
            if (line_Angle > 0 && abs(180-line_Angle) > threshold) {
                return PoseType.bodySlantL
            }
            if (line_Angle < 0 && abs(line_Angle+180) > threshold) {
                return PoseType.bodySlantR
            }
        }
        return PoseType.ok
    }
}
